/****************************************************************************
*  Title: laneDetector.sc
*  Author: Bryant Smith
*  Date: 05/03/2011
*  Description: Performs sobel corner detection on image array followed
*               by hough transform.  In a very simplistic attempt to perform
*               lane detection the transform is analized for the largest 
*               left and right leaning angles, these are averaged to output
*               a lane degree for the image (only the protion of the image
*               below the horizion is used)
****************************************************************************/

// TODO change sobel output to be bit instead of double

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


// define image size, idea being from a hw camera this would be fixed
#define IMG_ROWS 480
#define IMG_CLMS 640

import "c_queue";


/*
* Performs sobel edge detection and outputs a bit array of pixels identifiying edges
*/
// could split into sobel-x and sobel-y if placing on seperate hardware
// not done here, since this will be mapped to a single PE there is no sense in replicating loops and counters
behavior sobelEdge( in unsigned char image8bpp[IMG_ROWS][IMG_CLMS], out double sobelG[IMG_ROWS][IMG_CLMS])
{

	int masksize = 3;
	int sobelX[3][3] = {{-1, 0, 1}, {-2, 0, 2}, {-1, 0, 1}};
	int sobelY[3][3] = {{1, 2, 1}, {0, 0, 0}, {-1, -2, -1}};
	int x, y, x_1, y_1, posx, posy, imPosx, imPosy;
	int width, height;

	double Gx[IMG_ROWS][IMG_CLMS];
	double Gy[IMG_ROWS][IMG_CLMS];		
	double valx, valy, Gxy;

	void main(void) 
	{
	
		width = IMG_CLMS;
		height = IMG_ROWS;
		//********************
		// Apply sobel mask
		//********************			
		for(x = masksize / 2; x < width - (masksize / 2); x++) {  //columns
			for(y= masksize / 2; y < height- (masksize / 2); y++) { // rows
				valx = 0;
				valy = 0;
				for( x_1 = 0; x_1 < masksize; x_1++) {
					for( y_1 = 0; y_1 < masksize; y_1++) {
						imPosx = (x + (x_1 - (masksize / 2)));
						imPosy = (y + (y_1 - (masksize / 2)));
						
						valx += ((image8bpp[imPosy][imPosx]) * sobelX[y_1][x_1]);
						valy += ((image8bpp[imPosy][imPosx]) * sobelY[y_1][x_1]);
						//printf("valx %f\n", valx);
					}
				}
				
				// this is true sobel detection and create a much nicer looking picture
				// if output picture is desired behavior must output char array.
				//sobelG[y][x] =  sqrt(valx * valx + valy * valy);  // gradient magnitude
				
				// can approximate magniture and normalization for edge (Myler method)
				Gx[y][x] = valx * valx;  // horizontial gradient
				Gy[y][x] = valy * valy;  // vertical gradient
				
				Gxy = abs(valx) + abs(valy);
				
				//if (Gxy>255) Gxy=1;  // if using bit array				
				if (Gxy>255) {
					sobelG[y][x]=255;  // if using bits
				} else {
					sobelG[y][x] = 0;		
				}
			}
		}	
	}
};



/*
* hough transformation for very very rudimentry lane detection
* note this is computatinally intensive because of the exhaustive search space
*/
behavior houghLine(in double input_image[IMG_ROWS][IMG_CLMS], i_sender LaneOutQueue)
{
	unsigned char hough[360][961];  // we've pre set this based on our input image  2 * max + 1
	int r, i, j, rmax, omega, x, y, k, l, rows;
	int Xcenter, Ycenter, maxHough1, maxHough2, firstOmega, secondOmega, debug_temp;
	double rad;
	int LaneDegree[1];


	void main(void)
	{
		//only use 1/2 the image, disgard the top half as above the horizion
		//it will be ignored for lanes
		rows = IMG_ROWS/2;
	
		printf("Lane - Started lane detection\n");
		
		rad = 3.14159/180.0;
		Xcenter = IMG_CLMS / 2;  // columns center
		Ycenter = rows / 2;		 // rows center
		
		rmax = 342;  // can also pre determine this to 342 based on input image size

		// initilize hough image all black
		for (r = 0; r < 961; r++) {
			for (omega = 0; omega < 180; omega++) {
				hough[omega][r] = (double) 0;
			}
		}

		// create hough image from source image
		// note rows are interpreted backwards since bmp image is upside down
		for (j = rows; j > 0; j--) {
			for (i = 0; i < IMG_CLMS; i++) {
				if (input_image[j][i] == 255) {  //we've used a bit wise edge image
					for (omega = -180; omega < 180; omega++) {
						r = (i - Ycenter) * sin((double)(omega*rad)) + (j - Xcenter) * cos((double)(omega*rad));
						if (r > 0) {
							r++;
						}
						hough[omega][rmax+r] += 1;
					}
				}
			}
		}
		printf("Lane - performed hough transform\n");

		
		// find max hough to determine max omega angle on each side of lane
		// assumiing the lane is in teh center of the image and sides converge
		maxHough1 = 0;
		maxHough2 = 0;
		LaneDegree[0] = 0;
		
		// these quadrents need to be corrected.  for typical lane angels
		// transforming this image in matlab and viewing hough transform 
		// could help verify
		for (r = 0; r < 961; r++) {
			for (omega = 47; omega < 88; omega++) 
			{				
				if(maxHough1 < hough[omega][r])
				{
					maxHough1 = hough[omega][r];
					firstOmega = omega;
				}
			}
				for (omega = 86; omega < 133; omega++) 
			{				
				if(maxHough2 < hough[omega][r])
				{
					maxHough2 = hough[omega][r];
					secondOmega = omega;
				}
			}
		}		
		LaneDegree[0] = (firstOmega + secondOmega / 2)  -135;  // make 0 degrees straight ahead
		debug_temp = (firstOmega + secondOmega / 2) - 135;
		
		printf("Lane - finished lane detection lane degree: %d\n", debug_temp);
		LaneOutQueue.send(LaneDegree, 1);
		
	}

};



/*
* used to directly write out an image after normalizaing for debug purposes
*/
behavior debugSender(in double debug[IMG_ROWS][IMG_CLMS]) 
{
	// File names
	char OUTPUT_BMP[] = "output8bpp.bmp"; // output file name
	
	int row, wt_cnt, i, j;
	unsigned char pixel;
	double max, min, normFact;
	unsigned char edgeImage[IMG_ROWS][IMG_CLMS];
		
	void main(void) {
		
		//File variable
		FILE * wptr;
		
		// normalize
		// find min an max value
		max = 0;
		min = 0;
		for(i = 0; i < IMG_ROWS; i++) 
		{
			for(j = 0; j < IMG_CLMS; j++)
			{
				if ( debug[i][j] > max) max =  debug[i][j];
				if ( debug[i][j] < min) min =  debug[i][j];
			}
		}
		
		normFact = (max - min) / 255;
  
		// open output file file
		wptr = fopen(OUTPUT_BMP, "a"); // append to file which already has header from stimulus
  

		for(row = 0; row < IMG_ROWS; row++)
		{
			
			for(wt_cnt = 0; wt_cnt < IMG_CLMS; wt_cnt++)
			{
				pixel = (unsigned char) (( (debug[row][wt_cnt]) - min) / normFact);
				fprintf(wptr,"%c",pixel);                                    
				
				edgeImage[row][wt_cnt] = pixel;
			}
			
		}
  
	fclose(wptr);
	printf("debug image completed check file %s\n", OUTPUT_BMP);
	}
};

